package nxt.WallRobo_10;


import lejos.nxt.*;
public class Usthread extends Thread
{	
	public static double Drift = 0;
	public static int wallside = 0;
	public static boolean Map[][] = new boolean[100][100];
	public static int checkpoints = 0;
	public Object wait_for_wallside = new Object();
	public static int last_distance_distance = 0;
	public static int last_distance_Tacho = 0;
	public static int current_distance_distance = 0;
	public static int current_distance_Tacho = 0;
	int degrees = 0;
	public static boolean correction_is_needed = false;
	public UltrasonicSensor us = new UltrasonicSensor(SensorPort.S1);
	public Usthread(){}
	public void run()
	{
		
//Wandabfrage
		
		LCD.drawString("Wand R oder L?", 0, 0);

		while (!Button.RIGHT.isPressed() && !Button.LEFT.isPressed())
		{
		}

		if (Button.RIGHT.isPressed()) 
		{
			wallside = 1;
		} 
		else if (Button.LEFT.isPressed()) 
		{
			wallside = -1;
		}

		try
		{
			synchronized(wait_for_wallside)
			{	
				wait_for_wallside.notifyAll();
			}
		}
		catch (Exception e){}

		Motor.C.rotateTo(-90*wallside);	
		Motor.C.stop();


		while (true)
		{
			while(correction_is_needed==false)
			{
				for (int i = 0; i < 10; i++)
				{
					us.ping();
					Sound.pause(30);
					current_distance_distance = current_distance_distance
							+ us.getDistance();
					LCD.clear();
				}
	
				Sound.beep();
				current_distance_Tacho = Motor.A.getTachoCount();
				current_distance_distance = (int) current_distance_distance / 10;
				checkpoints = checkpoints + 1;
				Map[checkpoints-1][50] = true;
				if (last_distance_distance == 0)
				{
					last_distance_distance = current_distance_distance;
					last_distance_Tacho = current_distance_Tacho;
					LCD.drawInt(current_distance_distance, 0, 1);
					LCD.drawInt((int) (Drift), 0, 2);
					last_distance_distance = current_distance_distance;
					last_distance_Tacho = current_distance_Tacho;
					current_distance_distance = 0;
					current_distance_Tacho = 0;
	
				} else if (current_distance_distance != last_distance_distance)
				{
	
					Drift = (Math
							.toDegrees(Math
									.asin((current_distance_distance - last_distance_distance)
											/ ((current_distance_Tacho - last_distance_Tacho) * 0.0698))))
							* -wallside;
					
					correction_is_needed = true;
					if ((current_distance_distance - last_distance_distance)
							/ ((current_distance_Tacho - last_distance_Tacho) * 0.0698) >= 1
							|| (current_distance_distance - last_distance_distance)
									/ ((current_distance_Tacho - last_distance_Tacho) * 0.0698) <= -1)
					{
						//ecke; provisorisch:
						Sound.beepSequence();
						Sound.buzz();
						Motor.A.stop();
						Motor.B.stop();
						Motor.C.stop();
						System.exit(0);

						
					}else if (current_distance_distance==255)
					{
						Sound.beepSequence();
						Sound.buzz();
						Motor.A.stop();
						Motor.B.stop();
						Motor.C.stop();
						System.exit(0);
					}
					try
					{
						synchronized (Correctionthread.wait_for_correction)
						{
							Correctionthread.wait_for_correction.wait();
						}
					} catch (Exception e)
					{
					}

				}else{
					LCD.drawInt(current_distance_distance, 0, 1);
					LCD.drawInt((int) (Drift), 0, 2);
					last_distance_distance = current_distance_distance;
					last_distance_Tacho = current_distance_Tacho;
					current_distance_distance = 0;
					current_distance_Tacho = 0;
				}

				
			}

		}
	}
}
